#pragma config(UART_Usage, UART1, uartVEXLCD, baudRate19200, IOPins, None, None)
#pragma config(UART_Usage, UART2, uartNotUsed, baudRate4800, IOPins, None, None)
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in1,    peBatteryInput, sensorAnalog)
#pragma config(Sensor, in2,    armMeasure,     sensorPotentiometer)
#pragma config(Sensor, in3,    armSetPot,      sensorPotentiometer)
#pragma config(Sensor, in4,    gyro,           sensorGyro)
#pragma config(Sensor, I2C_1,  encoderRight,   sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_2,  encoderLeft,    sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port1,           backleft,      tmotorVex393HighSpeed, openLoop, reversed, encoder, encoderPort, I2C_2, 1000)
#pragma config(Motor,  port2,           frontleft,     tmotorVex393HighSpeed, openLoop)
#pragma config(Motor,  port3,           armbottomleft, tmotorVex269, openLoop, reversed)
#pragma config(Motor,  port4,           armtopleft,    tmotorVex393, openLoop)
#pragma config(Motor,  port7,           armbottomright, tmotorVex269, openLoop)
#pragma config(Motor,  port8,           armtopright,   tmotorVex393, openLoop, reversed)
#pragma config(Motor,  port9,           frontright,    tmotorVex393HighSpeed, openLoop, reversed)
#pragma config(Motor,  port10,          backright,     tmotorVex393HighSpeed, openLoop, reversed, encoder, encoderPort, I2C_1, 1000)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(15)
#pragma userControlDuration(105)

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!
#include "Driver.c"


//*******Pre Autonomous********//

void pre_auton(){
	StartTask(startLCD,6);
	displayOverride=true;
	setOverrideStrings(sInit,sNoMove);

  initEncoders();
  initGyro();
  initPIDs();

	displayOverride=false;
}//end pre-auton

task autonomous(){}

//*******User Control*********//

task usercontrol(){
	float kp=0;
	float ki=0;
	float kd=0;
	StartTask(dialAnArm);
	while (true)
	{
		//drive inputs
		walk=vexRT[Ch3];
		strafe=vexRT[Ch4];
		turn=vexRT[Ch1];

		//Arm inputs
		//armPower=0;

		//scale
		scaleInputs();


		//Drive
    driveMecanum(walk,strafe,turn);

     //Arm
    //armMove(armPower);



    while(vexRT[Btn7U]!=0){
    	stopAllMotors();
  	}

  	displayOverride=true;
  	string temp;
  	string PIDs;
		sprintf(temp,"%4.0f",getArmSet());
		sprintf(PIDs,"%4.0f",getArm());

  	setOverrideStrings(temp,PIDs);
		wait1Msec(15);
	}
}//end usercontrol
